package ch.areion.areionapp.engine;

import java.util.Timer;
import java.util.TimerTask;

import ch.areion.areionapp.communication.Monitor;
import ch.areion.areionapp.sensor.IUSSensor;
import ch.areion.common.message.Module;
import ch.areion.common.message.Type;
import ch.areion.common.message.Version;

import ioio.lib.api.IOIO;
import ioio.lib.api.TwiMaster;
import ioio.lib.api.exception.ConnectionLostException;

import static ch.areion.areionapp.engine.MotorDriver.Register.*;
import static ch.areion.areionapp.engine.MotorDriver.*;
import static ch.areion.areionapp.engine.WallFollowerState.*;
import static ch.areion.areionapp.core.Controller.*;

/**
 * Class to drive the motors of Areion.
 * 
 * @author RetoMueller
 * @version 1.0
 */
public class Engine implements IEngine {

	private MotorControl motor;
	private Timer timer;
	private IOIO ioio;
	private TwiMaster iic;
	private boolean isBusy;
	private Terminator terminator;
	private boolean followWall;
	private WallFollower follower;
	private IUSSensor uss;
	private static final int TWI_MODULE = 2;

	/**
	 * Constructor of the class Engine.
	 * @param ioio_ makes it possible to share the IOIO instance.
	 * @param uss_ makes it possible to share the us sensor instance.
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	public Engine(IOIO ioio_, IUSSensor uss_) throws ConnectionLostException,
	    InterruptedException {
		timer = new Timer();
		ioio = ioio_;
		uss = uss_;
		iic = ioio.openTwiMaster(TWI_MODULE, TwiMaster.Rate.RATE_400KHz, false);
		(motor = new MotorControl(this, ioio, iic)).start();

	}

	@Override
	public void disconnect() throws ConnectionLostException, InterruptedException {
		stop();
		motor.disconnect();
		timer.cancel();
		iic.close();
	}
	
	@Override
	public void stop() throws ConnectionLostException, InterruptedException {
		stopWallFollower();
		//isBusy = false;
		byte[] request = new byte[] { CONTROL.byt(), Control.MOTOR_STOP.set() };
		iic.writeRead(SLAVE_ADR, false, request, request.length, new byte[1], 0);
		Thread.sleep(WAIT_TIME);
	}

	/**
	 * Sets the speed of the Motor 1 and 2.
	 * @param speed speed 16 bit signed in [mm/s]
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	private void setSpeeds(int speedM1, int speedM2) throws ConnectionLostException, InterruptedException {
		byte[] bytesSpeedM1 = toBytes((short) (speedM1 * SPEED_CORRECTION));
		byte[] bytesSpeedM2 = toBytes((short) (speedM2 * SPEED_CORRECTION));
		byte[] request = new byte[] { M1_SPEED_HB.byt(), bytesSpeedM1[1], bytesSpeedM1[0],  bytesSpeedM2[1], bytesSpeedM2[0]};
		iic.writeRead(SLAVE_ADR, false, request, request.length, null, 0);
		Thread.sleep(WAIT_TIME);
	}
	
	/**
	 * Sets the speed of the Motor 1 and 2 the target ticks to reach. After which a stop occurs. This method blocks until finished.
	 * @param speed speed 16 bit signed in [mm/s]
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	public void setSpeedsAndTicks(int speedM1, int speedM2, int ticks) throws ConnectionLostException, InterruptedException {
		byte[] bytesSpeedM1 = toBytes((short) (speedM1 * SPEED_CORRECTION));
		byte[] bytesSpeedM2 = toBytes((short) (speedM2 * SPEED_CORRECTION));
		byte[] bytesTicks = toBytes((short) (ticks));
		byte[] request = new byte[] { M1_SPEED_HB.byt(), bytesSpeedM1[1], bytesSpeedM1[0],  bytesSpeedM2[1], bytesSpeedM2[0],bytesTicks[1],bytesTicks[0]};
		iic.writeRead(SLAVE_ADR, false, request , request.length, null, 0);
		waitUntilTickerFinished();
	}
	
	public void rotateRight(int speed, int time) throws ConnectionLostException, InterruptedException{
		setSpeeds(-speed, speed);
		terminate(time);
	}
	
	public void rotateLeft(int speed, int time) throws ConnectionLostException, InterruptedException{
		setSpeeds(speed, -speed);
		terminate(time);
	}
	
	/**
	 * This method blocks until the targeted ticks are reached.
	 * @param speed speed 16 bit signed in [mm/s]
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	private void waitUntilTickerFinished() throws ConnectionLostException, InterruptedException {
		byte[] response;
		do{
			Thread.sleep(WAIT_TIME);
			response  = new byte[1];
			iic.writeRead(SLAVE_ADR, false, STATUS.cmd(), 1, response ,response .length);
		}while(response [0] == Status.BUSY.set());
		Thread.sleep(WAIT_TIME);
	}

	@Override
	public void driveStraight(int speed) throws ConnectionLostException, InterruptedException {
		setSpeeds(speed, speed);
	}
	
	@Override
	public void driveStraightTicked(int speed, int ticks) throws ConnectionLostException, InterruptedException
	{
		stopWallFollower();
		setSpeedsAndTicks(speed, speed, ticks);
	}
	
	@Override
	public void driveRightTicked(int speed, int ticks) throws ConnectionLostException, InterruptedException
	{
		stopWallFollower();
		setSpeedsAndTicks(speed, 0, ticks);
	}
	
	@Override
	public void driveLeftTicked(int speed, int ticks) throws ConnectionLostException, InterruptedException
	{
		stopWallFollower();
		setSpeedsAndTicks(0, speed, ticks);
	}
	
	private void correctAngle(int speed, int ticks, double tolerance) throws ConnectionLostException,InterruptedException {
		boolean ok = false;
		double gapDelta;

		while(!ok) {
			gapDelta = uss.getDistanceUSSF(10) - (uss.getDistanceUSSB(10)); // links negativ
			
			if(gapDelta > 0 && gapDelta > tolerance) {
				setSpeedsAndTicks(speed, 0, ticks);
			} else if(gapDelta < 0 && gapDelta < -tolerance) {
				setSpeedsAndTicks(0, speed, ticks);
			} else {
				ok = true;
			}
		}
	}
	
	private void correctPosition(int speed, int gap, double tolerance,double gapMiddle)throws ConnectionLostException, InterruptedException {
		double gapDelta = Math.abs(gapMiddle - gap);
		if(gapDelta < 150){
			gapDelta = LINE_CORRECTION_FACTOR_FINE_TUNING * gapDelta;
		}else{
			gapDelta = LINE_CORRECTION_FACTOR * gapDelta;
		}
		
		if(gapMiddle < (gap - tolerance)) {
			setSpeedsAndTicks(0, speed, TICKS_CURVE_45_DEGREES);
			setSpeedsAndTicks(speed, speed, (int)(gapDelta));
			setSpeedsAndTicks(speed, 0, TICKS_CURVE_45_DEGREES);
		}else if(gapMiddle > (gap + tolerance)) {
			setSpeedsAndTicks(speed, 0, TICKS_CURVE_45_DEGREES);
			setSpeedsAndTicks(speed, speed, (int)(gapDelta));
			setSpeedsAndTicks(0, speed, TICKS_CURVE_45_DEGREES);
		}
	}
	
	@Override
	public void correctAngle() throws ConnectionLostException,InterruptedException {
		stopWallFollower();
		correctAngle(SPEED_SLOWEST, 100, ANGLE_TOLERANCE_EXACT);
	}
	
	@Override
	public void correctPosition(int gap) throws ConnectionLostException, InterruptedException {
		stopWallFollower();
		correctAngle(SPEED_SLOWEST, 90, ANGLE_TOLERANCE_EXACT);
		correctPosition(SPEED_SLOW, gap, LINE_TOLERANCE_EXACT,((uss.getDistanceUSSF() + uss.getDistanceUSSB()) / 2));
		correctAngle(SPEED_SLOWEST, 90, ANGLE_TOLERANCE_EXACT);
	}
	
	@Override
	public void correctPosition(int gap, int number) throws ConnectionLostException, InterruptedException {
		for(int i = 0; i < number ; i++){
			correctPosition(gap);
		}
	}
	
	@Override
	public void driveAlongWall(int speed) throws ConnectionLostException, InterruptedException {
		stopWallFollower();
		(follower = new WallFollower(speed, 0)).start();
	}
	
	@Override
	public void driveAlongLine(int speed, int gap) throws ConnectionLostException, InterruptedException {
		stopWallFollower();
		(follower = new WallFollower(speed, gap)).start();
	}

	@Override
	public void driveAlongWallTimed(int speed, int time) throws ConnectionLostException, InterruptedException {
		stopWallFollower();
		(follower = new WallFollower(speed, 0)).start();
		terminate(time);
	}
	
	@Override
	public void driveAlongLineTimed(int speed, int gap, int time) throws ConnectionLostException, InterruptedException {
		stopWallFollower();
		(follower = new WallFollower(speed, gap)).start();
		terminate(time);
	}

	/**
	 * This Thread lets Areion drive along a wall on the right side by trying to
	 * reach or hold the gap between him and the wall.
	 */
	class WallFollower extends Thread {
		private WallFollowerState state = NO_OPERATION;
		private boolean lineCorrected;
		private int speed;
		private int gap;
		private double gapF;
		private double gapB;
		private double gapDelta;
		double gapMiddle;

		public WallFollower(int speed, int gap) {
			if(gap == 0){
				lineCorrected = false;
			}else{
				lineCorrected = true;
			}
			this.speed = speed;
			this.gap = gap;
		}

		@Override
		public void run() {
			followWall = true;
			while(followWall) {
				gapF = uss.getDistanceUSSF();
				gapB = uss.getDistanceUSSB();
				gapDelta = gapF - gapB; // links negativ
				gapMiddle = ((gapF+gapB) / 2);
				
				try {
					// correct angle to right
					if(gapDelta > 0 && gapDelta > ANGLE_TOLERANCE) {
						if(state != ANGLE_RIGHT_CORRECTED) {
							setSpeeds(speed,speed / 20);
							state = ANGLE_RIGHT_CORRECTED;
						}
					// correct angle to left
					}
					else if(gapDelta < 0 && gapDelta < -ANGLE_TOLERANCE) {
						if(state != ANGLE_LEFT_CORRECTED) {
							setSpeeds(speed / 20, speed);
							state = ANGLE_LEFT_CORRECTED;
						}
					}
					else { // Line Holding
						if(lineCorrected) {
							correctPosition(speed, gap, LINE_TOLERANCE, gapMiddle);
							setSpeeds(speed, speed);
						}else if(state != DROVE_FORWARD) {
							setSpeeds(speed, speed);
							state = DROVE_FORWARD;
						}
					}
				}
				catch(Exception e) {
					Monitor.getInstance().add(Version.V01, Type.ERROR, Module.ENGINE,
							"WALLFOLLOWER ERROR");
				}
			}
		}
	}
	
	/**
	 * Stops the WallFollower Thread.
	 * @throws InterrruptedException When thread is interrupted.
	 */
	private void stopWallFollower() throws InterruptedException {
		followWall = false;
		if(follower != null) {
			follower.join();
		}
	}

	/**
	 * Creates a new Terminator to stop the robot after driving a curve with the
	 * acquired angle or after driving a certain distance forward.
	 */
	private void terminate(long time) throws InterruptedException {
		isBusy = true;
		if(terminator != null) {
			terminator.cancel();
		}
		timer.schedule(terminator = new Terminator(), time);
		waitUntilNotBusy();
	}
	
	private void waitUntilNotBusy() throws InterruptedException {
		while(isBusy);
	}

	/**
	 * TimerTask to stop the robot after driving a curve with the acquired angle
	 * or after driving a certain distance forward.
	 */
	class Terminator extends TimerTask {

		public void run() {
			try {
				stop();
				isBusy = false;
			}
			catch(Exception e) {

			}
		}
	}
}
